import random
import math
from operator import itemgetter, attrgetter
from dataclasses import dataclass
import logging
from typing import List
import os
import sys
import inspect

CURRENTDIR = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
PARENTDIR = os.path.dirname(CURRENTDIR)
sys.path.insert(0, CURRENTDIR)
sys.path.insert(0, PARENTDIR)

from common.rotationutils import getOrientation, getRotationMatrix

@dataclass
class b_bcontainer:
    a: str
    b: int
    c: int


@dataclass
class p_orientation:
    """
    When initializing: set the init values from outside
    """

    b: bool
    c: int
    d: int
    e: int
    f: List
    g: List
    h: List
    i: float
    j: float
    k: float

    def a(self, nvar):
        f2 = 0.0
        R, I = getRotationMatrix(nvar.a, nvar.b)
        array2 = getOrientation(R)
        degrees = math.degrees(array2[0])
        degrees2 = math.degrees(array2[1])
        degrees3 = math.degrees(array2[2])
        logging.debug(
            "Orientation pitch: {} roll: {} yaw: {}".format(degrees2, degrees3, degrees)
        )
        f3 = degrees * -1.0
        if f3 < 0.0:
            f3 += 360.0
        if self.i != 400.0:
            f2 = max(
                abs(self.i - f3), max(abs(self.j - degrees2), abs(self.k - degrees3))
            )
        self.i = f3
        self.j = degrees2
        self.k = degrees3
        return f2

    def update(self, data):
        self.c += 1
        if self.c >= 300:
            return
        nvar = data
        """
        this needs to be called when there's a new sensor event
        """
        if len(self.f) < 10:
            self.f.append(nvar)
        elif (nvar.c - self.e >= 1000 or nvar.d == 2) and len(self.h) < 10:
            self.h.append(nvar)
            self.e = nvar.c
            self.a(nvar)
        else:
            a2 = self.a(nvar)
            self.g.append({"a": nvar, "b": a2})
            if len(self.g) > 10:
                self.g.sort(key=lambda x: x["a"].c)
                self.g = self.g[:9]

    def b_b(self):
        arraylist = []
        for i in self.f:
            arraylist.append(i)
        for i in self.h:
            arraylist.append(i)
        for i in self.g:
            arraylist.append(i["a"])
        arraylist.sort(key=lambda x: x.c)
        st = ""
        j2 = 0
        j3 = int(self.d)
        j4 = j3
        for nvar in arraylist:
            R, I = getRotationMatrix(nvar.a, nvar.b)
            array2 = getOrientation(R)
            degrees = math.degrees(array2[1])
            degrees2 = math.degrees(array2[2])
            degrees3 = math.degrees(array2[0]) * -1.0
            if degrees3 < 0.0:
                degrees3 += 360.0
            format = str(round(degrees3, 2))
            format2 = str(round(degrees * -1.0, 2))
            format3 = str(round(degrees2, 2))
            j5 = int(nvar.c - j4)
            if j5 < 0:
                j5 = 0
            str2 = "{},{},{},{},{};".format(j5, format, format2, format3, nvar.d)
            logging.debug("Orientation Strinng: {}".format(str2))
            ro = j5 + int(float(format) + float(format2) + float(format3) * 100.0)
            st += str2
            j2 += ro
            j4 = nvar.c
        return b_bcontainer(a=st, b=j2, c=self.c)
